#include "CheckBoard.h"
#include <SensorUtility/sensorutility_global.h>
#include <glm/glm.hpp>
#include "opencv2/highgui/highgui.hpp"

CheckBoard::CheckBoard (int rows, int cols, float size) :
		_corners (rows * cols), _rows (rows), _cols (cols),
		_blockSize (size),
		_depthSegs (std::min (rows, cols) * (std::min (rows, cols) - 1) / 2)
{
	int index = 0;
	for (int v = 0; v < rows; ++v) {
		for (int u = 0; u < cols; ++u) {
			_corners[index] = cv::Point3f ((float)u * size, 0, (float)v * size);
			++index;
		}
	}
	for (int i = 0; i < std::min (rows, cols); ++i) {
		cv::Point3f startPoint = _corners[i * _cols + i];
		for (int j = 1; j < std::min (rows, cols) - i; ++j) {
			_corners.push_back (startPoint + cv::Point3f (
					0.0f, _blockSize * j, 0.0f));
		}
	}
}

CheckBoard::~CheckBoard ()
{

}

const std::vector< cv::Point3f >& CheckBoard::getCorners () const
{
	return _corners;
}

std::vector< cv::Point2f > CheckBoard::findCheckBoard (const QImage& img,
		bool& outisFound)
{
	outisFound = false;
	if (img.isNull ())
		return cv::Mat ();
	cv::Mat image (img.height (), img.width (),
			qImageFormat2CvImageType (img.format ()), (void*)img.bits ());
	//imshow ("...", image);
	std::vector< cv::Point2f > CornersFound;
	outisFound = cv::findChessboardCorners (image, cv::Size (_cols, _rows),
			CornersFound);
	if (outisFound) {
		int cornerCount = _rows * _cols;
		static cv::Mat grayPattImage (image.rows, image.cols, CV_8UC1);
		if (grayPattImage.size != image.size)
			grayPattImage = cv::Mat (image.rows, image.cols, CV_8UC1);
		cv::cvtColor (image, grayPattImage, CV_RGBA2GRAY);
		cv::cornerSubPix (grayPattImage, CornersFound,
				cv::Size (5, 5), cv::Size (1, 1),
				cv::TermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 30, 0.1));
		if (glm::length (glm::vec2 (CornersFound[0].x, CornersFound[0].y)) >
				glm::length (glm::vec2 (CornersFound[cornerCount - 1].x,
						CornersFound[cornerCount - 1].y))) {
			for (int i = 0; i < cornerCount / 2; ++i) {
				cv::Point2f temp = CornersFound[i];
				CornersFound[i] = CornersFound[cornerCount - 1 - i];
				CornersFound[cornerCount - 1 - i] = temp;
			}
		}
	}
	return CornersFound;
}

int CheckBoard::getRows () const
{
	return _rows;
}

int CheckBoard::getCols () const
{
	return _cols;
}

float CheckBoard::getBlockSize () const
{
	return _blockSize;
}

void CheckBoard::fillDepthSegs (std::vector< cv::Point3f >& io_points)
{

}

int CheckBoard::getCornersCount () const
{
	return _rows * _cols;
}

int CheckBoard::getDepthSegs () const
{
	return _depthSegs;
}
